using System;
using System.Drawing;
using Diversity.Maths;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// Used to add probabilities to a map. It uses a simple gaussian across the beam.
    /// </summary>
    public class ConeModel1Gaussian : IConeModel
    {
        private double _maxRange;
        private double _coneWidth;

        /// <summary>
        /// The width of the Cone in radians
        /// </summary>
        public double ConeWidth
        {
            get { return _coneWidth; }
            set { _coneWidth = value; }
        }
        private double _halfConeWidth;
        private TrigUtils _trig;
        private double[] _angleProbabilities;
        private double[] _truthProbabilities;
        private double[] _emptyProbabilities;
        private double _angleProbabilityGranularity;
        private double _oneOverAngleProbabilityGranularity;
        private double _angleVariance;
        private double _obstacleSize;

        public double ObstacleSize
        {
            get { return _obstacleSize; }
            set { _obstacleSize = value; }
        }
        private double _occupiedProb;
        private double _emptyProb;

        public double MaxRange
        {
            get { return _maxRange; }
            set { _maxRange = value; }
        }

        public double OccupiedProbability
        {
            get { return _occupiedProb; }
            set { _occupiedProb = value; }
        }

        public double EmptyProbability
        {
            get { return _emptyProb; }
            set { _emptyProb = value; }
        }
        
        public ConeModel1Gaussian( double maxRange, double coneWidth, double angleVariance, double obstacleSize, double occupiedProb, double emptyProb)
        {
            _trig = new TrigUtils();
            _maxRange = maxRange;
            _coneWidth = coneWidth;
            _halfConeWidth = coneWidth/2;
            _angleVariance = angleVariance;
            _obstacleSize = obstacleSize;
            _angleProbabilityGranularity = 0.01;
            _oneOverAngleProbabilityGranularity = 1.0/_angleProbabilityGranularity;
            _occupiedProb = occupiedProb;
            _emptyProb = emptyProb;

            // CREATE A LOOKUP TABLE OF THE PROBABILITIES OF ANGLES RELATIVE TO THE CENTER
            int numSteps = (int)Math.Round(coneWidth / ( _angleProbabilityGranularity));
            _angleProbabilities = ProbabilityUtils.CreateHalfDistribution(0, _angleVariance, _halfConeWidth, numSteps);
            _angleProbabilities = ProbabilityUtils.Normalize(_angleProbabilities, Normalization.MaxToOne);

            _truthProbabilities = new double[numSteps];
            _emptyProbabilities = new double[numSteps];
            for (int i = 0; i < _angleProbabilities.Length; i++)
            {
                double truth = (_occupiedProb * _angleProbabilities[i]) + 0.5;
                _truthProbabilities[i] = Math.Log(truth / (1 - truth));

                truth = 0.5 - (_emptyProb * _angleProbabilities[i]);
                _emptyProbabilities[i] = Math.Log(truth / (1 - truth));  
            }
        }

        public double[,] UpdateMap(double[, ] map, Pose2D state, double[] reading, double cmPerPixel, out Rectangle updateRect)
        {
           
            double readingTheta = _trig.CleanAngle(reading[1]);
            double oneOverCmPerPixel = 1.0/cmPerPixel;
            int rX = (int)Math.Round(state.X * oneOverCmPerPixel);
            int rY = (int)Math.Round(state.Y * oneOverCmPerPixel);
            int scaledDistance = (int)Math.Round(reading[0] * oneOverCmPerPixel);
            int scaledMaxDistance = (int)Math.Round((_maxRange) * oneOverCmPerPixel) - 1;
            int mapWidth = map.GetLength(0);
            int mapHeight = map.GetLength(1);
            double halfScaledObstacleSize = _obstacleSize/(2* cmPerPixel);
            double modelRange = scaledDistance + halfScaledObstacleSize;

            // find the box containing this beam
            int leftX = (int)Math.Round(rX + Math.Cos(readingTheta - _halfConeWidth) * modelRange);
            int leftY = (int)Math.Round(rY + Math.Sin(readingTheta - _halfConeWidth) * modelRange);
            int rightX = (int)Math.Round(rX + Math.Cos(readingTheta + _halfConeWidth) * modelRange);
            int rightY = (int)Math.Round(rY + Math.Sin(readingTheta + _halfConeWidth) * modelRange);
            int centerX = (int)Math.Round(rX + Math.Cos(readingTheta) * modelRange);
            int centerY = (int)Math.Round(rY + Math.Sin(readingTheta) * modelRange);
            int minBoxX = Math.Max(0, (Math.Min(Math.Min(Math.Min(rX, leftX), rightX), centerX)));
            int minBoxY = Math.Max(0, (Math.Min(Math.Min(Math.Min(rY, leftY), rightY), centerY)));
            int maxBoxX = Math.Min(mapWidth, Math.Max(Math.Max(Math.Max(rX, leftX), rightX), centerX));
            int maxBoxY = Math.Min(mapHeight, Math.Max(Math.Max(Math.Max(rY, leftY), rightY), centerY));
            //Console.Out.WriteLine("Box: " + minBoxX + "," + minBoxY + " " + maxBoxX + "," + maxBoxY);
            
            updateRect = new Rectangle(minBoxX, minBoxY, maxBoxX - minBoxX, maxBoxY - minBoxY);
            // update the cells
            for (int x = minBoxX; x < maxBoxX; x++)
            {
                for (int y = minBoxY; y < maxBoxY; y++)
                {
                    double r = Math.Sqrt(Math.Pow(rX - x, 2) + Math.Pow(rY - y, 2));
                    double fi = Math.Atan2(y - rY, x - rX);
                    double dTheta = Math.Abs(_trig.GetDifference(readingTheta, fi));
                    if (r > Math.Min(scaledMaxDistance - halfScaledObstacleSize, scaledDistance + halfScaledObstacleSize) || dTheta > _halfConeWidth)
                    {
                        // do nothing
                    }
                    else
                    {
                        // using a cached lookup for faster performance
                        if (r < scaledMaxDistance - halfScaledObstacleSize && Math.Abs(r - scaledDistance) < halfScaledObstacleSize)
                        {
                            map[x, y] += _truthProbabilities[(int)Math.Floor(dTheta * _oneOverAngleProbabilityGranularity)];// Math.Log(truth / (1-truth)); 
                        }
                        else
                        {
                            map[x, y] += _emptyProbabilities[(int)Math.Floor(dTheta * _oneOverAngleProbabilityGranularity)];// Math.Log(truth / (1-truth)); // Math.Log(truth / (1-truth));   
                        }
                    }   
               }
            }
            return map;
        }

        // same but using a distance reading object
        public void  UpdateMap(ref double[,] map, DistanceReading reading, double cmPerPixel)
        {
            double readingTheta =  _trig.CleanAngle(reading.Theta);
            double oneOverCmPerPixel = 1.0 / cmPerPixel;
            int mapWidth = map.GetLength(0);
            int mapHeight = map.GetLength(1);
            int rX = (int)Math.Round(reading.X * oneOverCmPerPixel);
            int rY = (int)Math.Round(reading.Y * oneOverCmPerPixel);
            int scaledDistance = (int)Math.Round(reading.Distance * oneOverCmPerPixel);
            int scaledMaxDistance = (int)Math.Round(_maxRange * oneOverCmPerPixel) - 1;
           
            double halfScaledObstacleSize = _obstacleSize / (2 * cmPerPixel);
            double modelRange = scaledDistance + halfScaledObstacleSize;
           
            // find the box containing this beam
            int leftX = (int) Math.Round(rX + Math.Cos(readingTheta - _halfConeWidth)*modelRange);
            int leftY = (int) Math.Round(rY + Math.Sin(readingTheta - _halfConeWidth)*modelRange);
            int rightX = (int) Math.Round(rX + Math.Cos(readingTheta + _halfConeWidth)*modelRange);
            int rightY = (int) Math.Round(rY + Math.Sin(readingTheta + _halfConeWidth)*modelRange);
            int centerX = (int) Math.Round(rX + Math.Cos(readingTheta)*modelRange);
            int centerY = (int) Math.Round(rY + Math.Sin(readingTheta)*modelRange);
            int minBoxX = Math.Max(0, (Math.Min(Math.Min(Math.Min(rX, leftX), rightX), centerX)));
            int minBoxY = Math.Max(0, (Math.Min(Math.Min(Math.Min(rY, leftY), rightY), centerY)));
            int maxBoxX = Math.Min(mapWidth, Math.Max(Math.Max(Math.Max(rX, leftX), rightX), centerX));
            int maxBoxY = Math.Min(mapHeight, Math.Max(Math.Max(Math.Max(rY, leftY), rightY), centerY));
            //  Console.Out.WriteLine("Box: " + minBoxX + "," + minBoxY + " " + maxBoxX + "," + maxBoxY);
            double maxTest =
                Math.Min(scaledMaxDistance - halfScaledObstacleSize, scaledDistance + halfScaledObstacleSize);
            //  updateRect = new Rectangle(minBoxX, minBoxY, maxBoxX - minBoxX, maxBoxY - minBoxY);
            // update the cells
            for (int x = minBoxX; x < maxBoxX; x++)
            {
                for (int y = minBoxY; y < maxBoxY; y++)
                {
                    double fi = Math.Atan2(y - rY, x - rX);
                    double dTheta = Math.Abs(_trig.GetDifference(readingTheta, fi));

                    if (dTheta <= _halfConeWidth)
                    {
                        double r = Math.Sqrt(Math.Pow(rX - x, 2) + Math.Pow(rY - y, 2));
                        if (r <= maxTest)
                        {
                            double change;
                            // double probOfScan = 0;
                            if (r < scaledMaxDistance - halfScaledObstacleSize &&
                                Math.Abs(r - scaledDistance) < halfScaledObstacleSize)
                            {
                                change =
                                    _truthProbabilities[(int) Math.Floor(dTheta*_oneOverAngleProbabilityGranularity)
                                        ];
                            }
                            else
                            {
                                change =
                                    _emptyProbabilities[(int) Math.Floor(dTheta*_oneOverAngleProbabilityGranularity)
                                        ];
                            }

                            map[x, y] += change;
                        }
                    }
                }
            }
            
        }

        // same without the ref
        public double[,] UpdateMap(double[,] map, DistanceReading reading, double cmPerPixel)
        {
            double readingTheta = _trig.CleanAngle(reading.Theta);
           // double halfConeWidth = _coneWidth / 2;
            int mapWidth = map.GetLength(0);
            int mapHeight = map.GetLength(1);
            double oneOverCmPerPixel = 1.0 / cmPerPixel;
            int rX = (int)Math.Round(reading.X * oneOverCmPerPixel);
            int rY = (int)Math.Round(reading.Y * oneOverCmPerPixel);
            int scaledDistance = (int)Math.Round(reading.Distance * oneOverCmPerPixel);
            int scaledMaxDistance = (int)Math.Round((_maxRange) * oneOverCmPerPixel) - 1;

            double halfScaledObstacleSize = _obstacleSize / (2 * cmPerPixel);
            double modelRange = scaledDistance + halfScaledObstacleSize;

            // find the box containing this beam
            int leftX = (int)Math.Round(rX + Math.Cos(readingTheta - _halfConeWidth) * modelRange);
            int leftY = (int)Math.Round(rY + Math.Sin(readingTheta - _halfConeWidth) * modelRange);
            int rightX = (int)Math.Round(rX + Math.Cos(readingTheta + _halfConeWidth) * modelRange);
            int rightY = (int)Math.Round(rY + Math.Sin(readingTheta + _halfConeWidth) * modelRange);
            int centerX = (int)Math.Round(rX + Math.Cos(readingTheta) * modelRange);
            int centerY = (int)Math.Round(rY + Math.Sin(readingTheta) * modelRange);
            int minBoxX = Math.Max(0, (Math.Min(Math.Min(Math.Min(rX, leftX), rightX), centerX)));
            int minBoxY = Math.Max(0, (Math.Min(Math.Min(Math.Min(rY, leftY), rightY), centerY)));
            int maxBoxX = Math.Min(mapWidth, Math.Max(Math.Max(Math.Max(rX, leftX), rightX), centerX));
            int maxBoxY = Math.Min(mapHeight, Math.Max(Math.Max(Math.Max(rY, leftY), rightY), centerY));
            //  Console.Out.WriteLine("Box: " + minBoxX + "," + minBoxY + " " + maxBoxX + "," + maxBoxY);
            double maxTest = Math.Min(scaledMaxDistance - halfScaledObstacleSize, scaledDistance + halfScaledObstacleSize);
            //  updateRect = new Rectangle(minBoxX, minBoxY, maxBoxX - minBoxX, maxBoxY - minBoxY);
            // update the cells
            for (int x = minBoxX; x < maxBoxX; x++)
            {
                for (int y = minBoxY; y < maxBoxY; y++)
                {
                    double fi = Math.Atan2(y - rY, x - rX);
                    double dTheta = Math.Abs(_trig.GetDifference(readingTheta, fi));

                    if (dTheta <= _halfConeWidth)
                    {
                        double r = Math.Sqrt(Math.Pow(rX - x, 2) + Math.Pow(rY - y, 2));
                        if (r <= maxTest)
                        {
                            double change;
                            // double probOfScan = 0;
                            if (r < scaledMaxDistance - halfScaledObstacleSize &&
                                Math.Abs(r - scaledDistance) < halfScaledObstacleSize)
                            {
                                // probOfScan = 0.8;
                                // try bayseian update
                                change = _truthProbabilities[(int)Math.Floor(dTheta * _oneOverAngleProbabilityGranularity)];
                            }
                            else
                            {
                                //probOfScan = 0.2;
                                change = _emptyProbabilities[(int)Math.Floor(dTheta * _oneOverAngleProbabilityGranularity)];
                            }
                            //change = Math.Log()

                            map[x, y] += change;
                        }

                    }
                }
            }
             return map;
        }
    }
}
